#include "main.h"
#include "PID.h"
#include <math.h>
void PID_param_init(PID_t *pid,float Kp,float Ki,float Kd,uint8_t outM,uint8_t outm)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->tar = 0;
    pid->act = 0;
    pid->err[0] = 0;
    pid->err[1] = 0;
    pid->err_int = 0;
    pid->out = 0;//输出为占空比
    pid->outM = outM;
}
void PID_calculate(PID_t *pid)
{
    //位置式
    //PID计算
    pid->err[0] = pid->tar - pid->act;
    pid->out =  pid->Kp*pid->err[0]
                +pid->err_int
                +pid->Kd*(pid->err[0]-pid->err[1]);
    //误差传递
    pid->err[1] = pid->err[0];
    //误差积分
    pid->err_int += pid->Ki*pid->err[0];
    //积分限幅 
    if(pid->err_int > pid->outM)pid->err_int = pid->outM;
    else if(pid->err_int < -pid->outM)pid->err_int = -pid->outM;
    //输出限幅
    if(pid->out > pid->outM)pid->out = pid->outM;
    else if(pid->out < -pid->outM)pid->out = -pid->outM;
}
